#include "ar_track_landing/ar_track_landing.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "yolo_detect_message/srv/service.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
ar_track::ar_track(/* args */):Node("ar_track_landing_node")
{

    // -------------------------------------------declare parameters-------------------------------------------
    this->declare_parameter<std::string>("subscribers.uav_state_sub.topic", "/mavros/state");
    this->declare_parameter<int>("subscribers.uav_state_sub.queue_size", 10);

    this->declare_parameter<std::string>("subscribers.uav_location_sub.topic", "/mavros/local_positon/odom");
    this->declare_parameter<int>("subscribers.uav_location_sub.queue_size", 10);

    this->declare_parameter<std::string>("subscribers.aros_pos_sb.topic", "/aruco_single/position");
    this->declare_parameter<int>("subscribers.aros_pos_sb.queue_size", 10);

    this->declare_parameter<std::string>("subscribers.velocity_pub.topic", "/mavros/setpoint_velocity/cmd_vel_unstamped");

    this->declare_parameter<std::string>("client.arming_client.topic", "/mavros/cmd/arming");
    this->declare_parameter<std::string>("client.set_mode_client.topic", "/mavros/set_mode");
    
    this->declare_parameter<std::string>("client.velocity_pub.topic", "/mavros/setpoint_velocity/cmd_vel_unstamped");
    this->declare_parameter<std::string>("subscribers.locaitonsub.topic", "/mavros/setpoint_velocity/cmd_vel_unstamped");

    this->declare_parameter<std::string>("services.calldetectserver.topic", "ar_track_landing_server");


    this->declare_parameter<double>("parameters.max_value", 0.5);
    this->declare_parameter<std::string>("subscribers.call_server.topic","ar_track_landing_server_topic");
    this->declare_parameter<double>("parameters.max_error", 0.2);
    this->declare_parameter<double>("parameters.vel_value", 1.0);

    this->declare_parameter<int>("parameters.vehicle_id", 1);
    this->declare_parameter<std::string>("parameters.namesspace", "drone_2");

    // -------------------------------------------reader parameters-------------------------------------------
    this->get_parameter("parameters.namesspace", namespaces);
    this->get_parameter("subscribers.uav_state_sub.topic", uav_state_topic);
    uav_state_topic = namespaces + uav_state_topic;
    RCLCPP_INFO(this->get_logger(), "UAV State Topic: %s", uav_state_topic.c_str());

    this->get_parameter("subscribers.uav_state_sub.queue_size", uav_state_queue_size);

    this->get_parameter("subscribers.uav_location_sub.topic", uav_location_topic);
    uav_location_topic = namespaces + uav_location_topic;
    RCLCPP_INFO(this->get_logger(), "UAV Location Topic: %s", uav_location_topic.c_str());
    this->get_parameter("subscribers.uav_location_sub.queue_size", uav_location_queue_size);

    this->get_parameter("subscribers.aros_pos_sb.topic", aruco_position_topic);
    RCLCPP_INFO(this->get_logger(), "Aruco Position Topic: %s", aruco_position_topic.c_str());
    this->get_parameter("subscribers.aros_pos_sb.queue_size", aruco_position_queue_size);

    this->get_parameter("subscribers.velocity_pub.topic", velocity_pub_topic);
    velocity_pub_topic = namespaces + velocity_pub_topic;
    RCLCPP_INFO(this->get_logger(), "Velocity Pub Topic: %s", velocity_pub_topic.c_str());
    this->get_parameter("client.arming_client.topic", arming_client_topic);
    arming_client_topic = namespaces + arming_client_topic;
    RCLCPP_INFO(this->get_logger(), "Arming Client Topic: %s", arming_client_topic.c_str());
    this->get_parameter("client.set_mode_client.topic", set_mode_client_topic);
    RCLCPP_INFO(this->get_logger(), "Set Mode Client Topic: %s", set_mode_client_topic.c_str());
    this->get_parameter("client.velocity_pub.topic", velocity_pub_topic);

    this->get_parameter("services.calldetectserver.topic", detect_server_service);
    RCLCPP_INFO(this->get_logger(), "Detect Server Service: %s", detect_server_service.c_str());

    this->get_parameter("parameters.max_value", max_value);
    this->get_parameter("parameters.max_error", MAX_ERROR);
    this->get_parameter("parameters.vel_value", VEL_SET );
    this->get_parameter("subscribers.call_server.topic", call_server_topic_name);
    call_server_topic_name = namespaces + call_server_topic_name;

    this->get_parameter("parameters.vehicle_id", vehcile_id_);

    // Now you can use these variables in your code
    RCLCPP_INFO(this->get_logger(), "UAV State Topic: %s, Queue Size: %d", uav_state_topic.c_str(), uav_state_queue_size);
    RCLCPP_INFO(this->get_logger(), "Max Value Parameter: %f", max_value);
    RCLCPP_INFO(this->get_logger(), "namespaces: %s", namespaces.c_str());

    // -------------------------------------------create subscribers and clients-------------------------------------------
    if (namespaces.empty())
    {
        rclcpp::shutdown();
    }
    

    xy_is_arrived = false;
  
    state_sub_ = this->create_subscription<mavros_msgs::msg::State>(
            uav_state_topic, 10, std::bind(&ar_track::state_callback, this, std::placeholders::_1));

    local_pos_sb = this->create_subscription<nav_msgs::msg::Odometry>(
            uav_location_topic,100,std::bind(&ar_track::local_pos_cb,this,std::placeholders::_1));

    ar_pos_sb = this->create_subscription<geometry_msgs::msg::Vector3Stamped>(
            aruco_position_topic,rclcpp::QoS(10).best_effort(),std::bind(&ar_track::ar_pos_cb,this,std::placeholders::_1)
    );
    
    arming_client_ = this->create_client<mavros_msgs::srv::CommandBool>(namespaces + arming_client_topic);

    set_mode_client_ = this->create_client<mavros_msgs::srv::SetMode>(namespaces + set_mode_client_topic);

    mavros_velocity_pub =  this->create_publisher<geometry_msgs::msg::Twist>(velocity_pub_topic, 10);
    
    // 统一接口的速度发布
    std::string move_pub_str_ = "/uav_swarms/" + namespaces+ "/move";
    move_pub_ = this->create_publisher<yolo_detect_message::msg::MoveInterface>(move_pub_str_,10);

    std::string cmd_pub_str_ = "/uav_swarms/" + namespaces+ "/cmd";
    cmd_pub_ = this->create_publisher<std_msgs::msg::String>(cmd_pub_str_,10);
    timer_ = this->create_wall_timer(std::chrono::milliseconds(50),std::bind(&ar_track::timer_loop,this));

    callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    detect_server_service = call_server_topic_name = call_server_topic_name + "/service";
    service_ = this->create_service<yolo_detect_message::srv::Service>(detect_server_service, std::bind(&ar_track::service_callback,this,std::placeholders::_1,std::placeholders::_2),rmw_qos_profile_services_default,callback_group_);
    service_status_subscriber_ = this->create_subscription<yolo_detect_message::msg::CallServerStatus>(call_server_topic_name,rclcpp::QoS(20).best_effort(),std::bind(&ar_track::service_status_callback,this,std::placeholders::_1));
}

ar_track::~ar_track()
{
}

// callback of px4 state
void ar_track::state_callback(const mavros_msgs::msg::State::SharedPtr msg){
    std::lock_guard<std::mutex> lock(current_state_mutex);
    current_state = *msg;
}

void ar_track::arm_drone()
{
    if (!arming_client_->wait_for_service(std::chrono::seconds(1)))
    {
        RCLCPP_ERROR(this->get_logger(), "Arming service not available");
        return;
    }

    auto request = std::make_shared<mavros_msgs::srv::CommandBool::Request>();
    request->value = true;  // 发送ARM命令

    // 异步发送请求，并使用回调函数处理结果
    auto result_future = arming_client_->async_send_request(request,
        [this](rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedFuture result) {
            if (result.get()->success)
            {
                RCLCPP_INFO(this->get_logger(), "Drone armed successfully");
            }
            else
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to arm drone");
            }
        });
}

void ar_track::set_offboard_mode()
{
    if (!set_mode_client_->wait_for_service(std::chrono::seconds(1)))
    {
        RCLCPP_ERROR(this->get_logger(), "Set mode service not available");
        return;
    }

    auto request = std::make_shared<mavros_msgs::srv::SetMode::Request>();
    request->custom_mode = "OFFBOARD";

    // 异步发送请求，并使用回调函数处理结果
    auto result_future = set_mode_client_->async_send_request(request,
        [this](rclcpp::Client<mavros_msgs::srv::SetMode>::SharedFuture result) {
            if (result.get()->mode_sent)
            {
                RCLCPP_INFO(this->get_logger(), "OFFBOARD mode enabled");
            }
            else
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to enable OFFBOARD mode");
            }
        });
}

void  ar_track::local_pos_cb(const nav_msgs::msg::Odometry::SharedPtr msg){
    local_pose = *msg;
    if (flag_init_position == false && (local_pose.pose.pose.position.z != 0))
    {
        init_position_x_take_off = local_pose.pose.pose.position.x;
        init_position_y_take_off = local_pose.pose.pose.position.y;
        init_position_z_take_off = local_pose.pose.pose.position.z;
        flag_init_position =true;
    }
    // no used
    geometry_msgs::msg::Quaternion quat_msg = local_pose.pose.pose.orientation;
    tf2::Quaternion quat;
    tf2::fromMsg(quat_msg,quat);
    double roll,pitch,yaw;
    tf2::Matrix3x3(quat).getRPY(roll,pitch,yaw);

}

// 二维码的位置
void  ar_track::ar_pos_cb(const geometry_msgs::msg::Vector3Stamped::SharedPtr msg){
    
    position_detect_x = msg->vector.x;
    position_detect_y = msg->vector.y;
    position_detect_z = msg->vector.z;
    first_deteced = true;

    // RCLCPP_INFO(this->get_logger(),"position_detect_x = %f",position_detect_x);
    // RCLCPP_INFO(this->get_logger(),"position_detect_y = %f",position_detect_y);
    // RCLCPP_INFO(this->get_logger(),"position_detect_z = %f",position_detect_z);
    {
        std::lock_guard<std::mutex> lock(wait_times_mutex);
        wait_times = 0;
    }
    
}

void ar_track::timer_loop()
{

    if (service_status.load() == false || first_deteced == false)
    {
        RCLCPP_ERROR(this->get_logger()," %s landing mission not started",namespaces);
        return;
    }
    if (wait_times > 15)
    {
        reset_velocity();
    }else{
        if(fabs(position_detect_x) < MAX_ERROR && fabs(position_detect_y) < MAX_ERROR &&xy_is_arrived ==false)
        {
            RCLCPP_INFO(this->get_logger(), "UAV ARRIVED");
            flag_arrive = true;
            // set_velocity_z(-0.5);
            xy_is_arrived = true;
            {
                std::lock_guard<std::mutex> wait_times_mutex_lock(wait_times_mutex);
                wait_times = 0;
            }
            
        }else if ((fabs(position_detect_x) > MAX_ERROR || fabs(position_detect_y) > MAX_ERROR) &&xy_is_arrived ==false )
        {
            //摄像头朝下安装，因此摄像头的Z对应无人机的X前后方向，Y对应Y左右方向，Z对应上下
            //无人机左右移动速度控制

            float velocity_x = 0.0;
            float velocity_y = 0.0;
            if(position_detect_x >= MAX_ERROR)
            {
                velocity_y = -1 * 1.0 *VEL_SET;
                current_velocity.linear.set__y(-1.0 *VEL_SET);
            }					
            else if(position_detect_x <= -MAX_ERROR)
            {
                velocity_y = 1 * 1.0 *VEL_SET;
            
            }	
            else
            {
                velocity_y = 0.0;
            }								  
            //无人机前后移动速度控制
            if(position_detect_y > MAX_ERROR)
            {
                velocity_x = -1 * 1.0 *VEL_SET;
            }
            else if(position_detect_y < -MAX_ERROR)
            {
                velocity_x = 1 * 1.0 *VEL_SET;
            }
            else
            {
                velocity_x = 0.0;
            }
         
            RCLCPP_INFO(this->get_logger(),"uav difference locate:x = %f,y:= %f",position_detect_x,position_detect_y);
            RCLCPP_INFO(this->get_logger(),"uav difference velocity:x = %f,y:= %f",velocity_x,velocity_y);
            set_velocity(velocity_x,velocity_y);
        }
    }
    if (first_deteced == false)
    {
        reset_velocity();
    }
    
    {
        std::lock_guard<std::mutex> wait_times_mutex_lock(wait_times_mutex);
        if (flag_arrive == false)
        {
            wait_times++;
        }
    }
    if (xy_is_arrived == true  && current_state.mode != "AUTO.LAND")
    {
        // set_velocity_z(-0.5);
        std_msgs::msg::String msg;
        msg.data = "AUTO.LAND";
        cmd_pub_->publish(msg);

    }else{
        yolo_detect_message::msg::MoveInterface msg;
        msg.is_body = true;
        msg.is_vel = true;
        msg.is_gps =false;
        msg.pose = geometry_msgs::msg::Pose();
        msg.vel = current_velocity;
        move_pub_->publish(msg);
        RCLCPP_INFO(this->get_logger(),"uav publish velocity:x = %f,y:= %f,z:= %f",current_velocity.linear.x,current_velocity.linear.y,current_velocity.linear.z);

    }
    
    
}


void ar_track::reset_velocity(){
    std::lock_guard<std::mutex> current_velocity_mutex_lock(current_velocity_mutex);
    current_velocity.angular.set__x(0.0);
    current_velocity.angular.set__y(0.0);
    current_velocity.angular.set__z(0.0);

    current_velocity.linear.set__x(0.0);
    current_velocity.linear.set__y(0.0);
    current_velocity.linear.set__z(0.0);
}



void ar_track::set_velocity(float x, float y){
    std::lock_guard<std::mutex> current_velocity_mutex_lock(current_velocity_mutex);
    current_velocity.angular.set__x(0.0);
    current_velocity.angular.set__y(0.0);
    current_velocity.angular.set__z(0.0);

    current_velocity.linear.set__x(x);
    current_velocity.linear.set__y(y);
    current_velocity.linear.set__z(0.0);
}

float ar_track::clamp(float value,float max_values){
    float result= {};
    if (value > max_value)
    {
        result = max_value;
    }
    if (value < (-1.0 * max_value))
    {
        result = max_value * -1.0;
    }
    return result;
    
    
}

void ar_track::set_velocity_z(float z){
    std::lock_guard<std::mutex> current_velocity_mutex_lock(current_velocity_mutex);
    current_velocity.angular.set__x(0.0);
    current_velocity.angular.set__y(0.0);
    current_velocity.angular.set__z(0.0);

    current_velocity.linear.set__x(0.0);
    current_velocity.linear.set__y(0.0);
    current_velocity.linear.set__z(z);
}


void ar_track::service_callback(const yolo_detect_message::srv::Service::Request::SharedPtr req,
                        const yolo_detect_message::srv::Service::Response::SharedPtr res){
    bool reqs = req->status;
    if(reqs){
        RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] service callback.");
    }
    else{
        RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] service callback false.");

    }
    service_status.store(reqs);
    
    res->success = true;
    res->drone_statue = "yolo detect service_status is " + std::to_string(service_status.load());
}

void ar_track::service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg){
  bool reqs = msg->status;
  service_status.store(reqs);
  RCLCPP_INFO(this->get_logger(),"[target tracker node] service status is %d.",service_status.load());
}